#-*- coding: utf-8 -*-
import rospy
import tf
from geometry_msgs.msg import PoseStamped

def get_current_pose():
    listener = tf.TransformListener()
    listener.waitForTransform('/map', '/base_link', rospy.Time(), rospy.Duration(4.0))
    (trans, rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0))

    current_pose = {
    'x': trans[0],
    'y': trans[1],
    'orientation': tf.transformations.euler_from_quaternion(rot)[2] # 获取 yaw 角
    }
    return current_pose

def goal_callback(msg):
    goal_pose = {
    'x': msg.pose.position.x,
    'y': msg.pose.position.y,
    'orientation': tf.transformations.euler_from_quaternion([
    msg.pose.orientation.x,
    msg.pose.orientation.y,
    msg.pose.orientation.z,
    msg.pose.orientation.w
    ])[2]
    }
    rospy.loginfo("Goal Pose: %s", goal_pose)

if __name__ == '__main__':
    rospy.init_node('get_pose')

    # 订阅目标点话题
    rospy.Subscriber('/move_base_simple/goal', PoseStamped, goal_callback)

    # 获取小车的当前位姿
    current_pose = get_current_pose()
    rospy.loginfo("Current Pose: %s", current_pose)

    rospy.spin()